

#define BOOST_TEST_MODULE MMMTools_MMM_ConverterSetupTest
#include <Test.h>

#include <MMMSimoxTools/MMMSimoxTools.h>
#include <VirtualRobot/Nodes/Sensor.h>

#include "../MMMConverterConfiguration.h"
#include <boost/test/unit_test.hpp>

#include <MMM/Motion/Motion.h>
#include <MMM/Model/Model.h>

#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessorWinter.h>
#include <MMM/Motion/MotionReaderC3D.h>
#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/MarkerMotion.h>
#include <MMM/Motion/MarkerData.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>
#include <MMM/Motion/Legacy/Converter/MarkerBasedConverter.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

// BOOST_AUTO_TEST_SUITE(XMLTest)

// TODO: Disabled all tests, should be re-enabled with well-defined paths here instead of relying on some kind of default configuration

/*

BOOST_AUTO_TEST_CASE(testSetupConverterFactories)
{
    MMMConverterConfiguration c;
    setupConverterFactories(c.converterLibSearchPaths);
	c.print();    
    std::vector< std::string > factoryList = MMM::ConverterFactory::getSubclassList();
	for (std::vector<std::string>::iterator it = factoryList.begin(); it!=factoryList.end(); it++)
        BOOST_TEST_MESSAGE(*it);

    // at least RidgdBody and Vicon2MMM must be present
    BOOST_REQUIRE(factoryList.size()>=2);
}


BOOST_AUTO_TEST_CASE(testSetupModelProcessor)
{
    MMMConverterConfiguration c;
    MMM::ModelProcessorFactoryPtr modelFactory = MMM::ModelProcessorFactory::fromName(c.modelProcessorName, NULL);
    BOOST_REQUIRE(modelFactory);

    MMM::ModelProcessorPtr modelProcessor = modelFactory->createModelProcessor();
    bool setupOK = modelProcessor->setupFile(c.modelProcessorConfigFile);
    BOOST_REQUIRE(setupOK);
}

BOOST_AUTO_TEST_CASE(testSetupConverter)
{
    MMMConverterConfiguration c;
    setupConverterFactories(c.converterLibSearchPaths);

    MMM::ConverterFactoryPtr converterFactory = MMM::ConverterFactory::fromName(c.converterName, NULL);
    BOOST_REQUIRE(converterFactory);

    MMM::ConverterPtr converterBase = converterFactory->createConverter();
    BOOST_REQUIRE(converterBase);
    MMM::MarkerBasedConverterPtr converter = boost::dynamic_pointer_cast<MMM::MarkerBasedConverter>(converterBase);
    BOOST_REQUIRE(converter);

    bool setupOK = converter->setupFile(c.converterConfigFile);
    BOOST_REQUIRE(setupOK);
    
    std::map<std::string, std::string> markerMapping = converter->getMarkerMapping();
    BOOST_REQUIRE(markerMapping.size()>20);
}


BOOST_AUTO_TEST_CASE(testLoadModel)
{
    MMMConverterConfiguration c;

    MMM::ModelPtr mmmOrigModel;
    try
    {
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        BOOST_REQUIRE(r);
        mmmOrigModel = r->loadModel(c.outModelFile);
        BOOST_REQUIRE(mmmOrigModel);
    }
    catch (...)
    {
        
    }
    BOOST_REQUIRE(mmmOrigModel);
}




BOOST_AUTO_TEST_CASE(testConvertModel)
{
    MMMConverterConfiguration c;
    setupConverterFactories(c.converterLibSearchPaths);

    MMM::ModelProcessorFactoryPtr modelFactory = MMM::ModelProcessorFactory::fromName(c.modelProcessorName, NULL);
    BOOST_REQUIRE(modelFactory);

    MMM::ModelProcessorPtr modelProcessor = modelFactory->createModelProcessor();
    bool setupOK = modelProcessor->setupFile(c.modelProcessorConfigFile);
    BOOST_REQUIRE(setupOK);
    MMM::ModelPtr mmmOrigModel;
    try
    {
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        BOOST_REQUIRE(r);
        mmmOrigModel = r->loadModel(c.outModelFile);
        BOOST_REQUIRE(mmmOrigModel);
    }
    catch (...)
    {
    }
    BOOST_REQUIRE(mmmOrigModel);

    MMM::ModelPtr mmmModel = modelProcessor->convertModel(mmmOrigModel);
    BOOST_REQUIRE(mmmModel);

    float height1 = mmmOrigModel->getHeight();
    BOOST_CHECK_EQUAL(height1,1.0f);
    float height2 = mmmModel->getHeight();
    //BOOST_CHECK_EQUAL(height2,1.79f);

    // convert to simox model
    VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(mmmModel);
    BOOST_REQUIRE(robot);
}


BOOST_AUTO_TEST_CASE(testLoadVicon)
{
    MMMConverterConfiguration c;
    MMM::MotionReaderC3DPtr c3d(new MMM::MotionReaderC3D());
    BOOST_REQUIRE(c3d);
    MMM::MarkerMotionPtr markerMotion = c3d->loadC3D(c.viconInputFile);
    BOOST_REQUIRE(markerMotion);
    BOOST_CHECK_GT(int(markerMotion->getNumFrames()),50);
    std::vector<std::string> markerLabels = markerMotion->getMarkerLabels();
    BOOST_REQUIRE(markerLabels.size()>10);
}


float getAvgMarkerDist(std::map<std::string, std::string>& markerMapping, MMM::MarkerDataPtr f, std::map<std::string, VirtualRobot::SensorPtr>& modelMarkers)
{
    std::map<std::string, std::string>::iterator it = markerMapping.begin();
    float res = 0;
    if (markerMapping.size()==0)
        return 0.0f;
    while (it != markerMapping.end())
    {
        std::string c3dMarker = it->first;
        std::string mmmMarker = it->second;

        Eigen::Vector3f posC3D = f->data[c3dMarker];
        Eigen::Vector3f posMMM = modelMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);
        res += (posMMM-posC3D).norm();
        it++;
    }
    res /= float(markerMapping.size());
    return res;
}

BOOST_AUTO_TEST_CASE(testConversion)
{
    MMMConverterConfiguration c;
    setupConverterFactories(c.converterLibSearchPaths);

    // load c3d
    MMM::MotionReaderC3DPtr c3d(new MMM::MotionReaderC3D());
    BOOST_REQUIRE(c3d);
    MMM::MarkerMotionPtr markerMotion = c3d->loadC3D(c.viconInputFile);
    BOOST_REQUIRE(markerMotion);

    // load model
    MMM::ModelProcessorFactoryPtr modelFactory = MMM::ModelProcessorFactory::fromName(c.modelProcessorName, NULL);
    BOOST_REQUIRE(modelFactory);
    MMM::ModelProcessorPtr modelProcessor = modelFactory->createModelProcessor();
    bool setupOK = modelProcessor->setupFile(c.modelProcessorConfigFile);
    BOOST_REQUIRE(setupOK);
    MMM::ModelPtr mmmOrigModel;
    try
    {
        MMM::ModelReaderXMLPtr r(new MMM::ModelReaderXML());
        BOOST_REQUIRE(r);
        mmmOrigModel = r->loadModel(c.outModelFile);
        BOOST_REQUIRE(mmmOrigModel);
    }
    catch (...)
    {
    }
    BOOST_REQUIRE(mmmOrigModel);
    MMM::ModelPtr mmmModel = modelProcessor->convertModel(mmmOrigModel);
    BOOST_REQUIRE(mmmModel);
    VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(mmmModel);
    BOOST_REQUIRE(robot);

    // store markers 
    std::map<std::string, VirtualRobot::SensorPtr> modelMarkers;
    std::vector<VirtualRobot::SensorPtr> sensors = robot->getSensors();
    for (size_t i = 0; i < sensors.size(); i++)
        modelMarkers[sensors[i]->getName()] = sensors[i];
    BOOST_REQUIRE(modelMarkers.size()>10);

    // converter
    MMM::ConverterFactoryPtr converterFactory = MMM::ConverterFactory::fromName(c.converterName, NULL);
    BOOST_REQUIRE(converterFactory);
    MMM::ConverterPtr converterBase = converterFactory->createConverter();
    BOOST_REQUIRE(converterBase);
    MMM::MarkerBasedConverterPtr converter = boost::dynamic_pointer_cast<MMM::MarkerBasedConverter>(converterBase);
    BOOST_REQUIRE(converter);
    setupOK = converter->setupFile(c.converterConfigFile);
    BOOST_REQUIRE(setupOK);

    setupOK = converter->setup(MMM::ModelPtr(), markerMotion, mmmModel);
    BOOST_REQUIRE(setupOK);

    std::map<std::string, std::string> markerMapping = converter->getMarkerMapping();
    BOOST_REQUIRE(markerMapping.size()>20);
    std::vector<std::string> markerLabels = markerMotion->getMarkerLabels();
    BOOST_REQUIRE(markerLabels.size()>10);
    std::vector<std::string> joints = converter->getJointOrder();
    BOOST_REQUIRE(joints.size()>10);

    // check joints
    for (size_t i = 0; i < joints.size(); i++)
    {
        BOOST_REQUIRE(robot->hasRobotNode(joints[i]));
    }

    // check markermapping
    MMM::MarkerDataPtr f = markerMotion->getFrame(0);
    BOOST_CHECK(f);
    std::map<std::string, std::string>::iterator it = markerMapping.begin();
    while (it != markerMapping.end())
    {
        std::string c3dMarker = it->first;
        std::string mmmMarker = it->second;
        BOOST_CHECK(f->data.find(c3dMarker) != f->data.end());
        BOOST_CHECK(modelMarkers.find(mmmMarker) != modelMarkers.end());
        //Eigen::Vector3f posC3D = f->data[c3dMarker];
        //Eigen::Vector3f posMMM = modelMarkers[mmmMarker]->getGlobalPose().block(0, 3, 3, 1);
        it++;
    }

    // initialize converter
    MMM::AbstractMotionPtr m = converter->initializeStepwiseConversion();
    BOOST_CHECK(m);
    MMM::MotionPtr resultModelMotion = boost::dynamic_pointer_cast<MMM::Motion>(m);
    BOOST_CHECK(resultModelMotion);
    robot->setGlobalPose(converter->getInitialModelFitting());
    robot->setJointValues(converter->getInitialJointFitting());
    f = markerMotion->getFrame(0);
    float avgMarkerDist = getAvgMarkerDist(markerMapping,f,modelMarkers);
    BOOST_CHECK_LT(avgMarkerDist,60.0f); // currently the error should be around 3 cm

    // convert frames "step by step"
    bool convertOK;
    std::map<std::string,float> jointValueMap;
    for (unsigned int i=0;i<markerMotion->getNumFrames();i++)
    {
        convertOK = converter->convertMotionStep(resultModelMotion);
        BOOST_CHECK(convertOK);
        MMM::MotionFramePtr mf = resultModelMotion->getMotionFrame(i);
        BOOST_CHECK(mf);
        robot->setGlobalPose(mf->getRootPose());
        BOOST_CHECK (mf->joint.rows() == joints.size());
        for (size_t j = 0; j < joints.size(); j++)
        {
            jointValueMap[joints[j]] = mf->joint[j];
        }

        robot->setJointValues(jointValueMap);

        // check result
        f = markerMotion->getFrame(i);
        avgMarkerDist = getAvgMarkerDist(markerMapping,f,modelMarkers);
        BOOST_CHECK_LT(avgMarkerDist,100.0f);
    }

    // convert all
    MMM::AbstractMotionPtr m2 = converter->convertMotion();
    BOOST_CHECK(m2);
    MMM::MotionPtr resultModelMotion2 = boost::dynamic_pointer_cast<MMM::Motion>(m2);
    BOOST_CHECK(resultModelMotion2);
    for (unsigned int i=0;i<markerMotion->getNumFrames();i++)
    {
        MMM::MotionFramePtr mf = resultModelMotion2->getMotionFrame(i);
        BOOST_CHECK(mf);
        robot->setGlobalPose(mf->getRootPose());
        BOOST_CHECK (mf->joint.rows() == joints.size());
        for (size_t j = 0; j < joints.size(); j++)
        {
            jointValueMap[joints[j]] = mf->joint[j];
        }

        robot->setJointValues(jointValueMap);

        // check result
        f = markerMotion->getFrame(i);
        avgMarkerDist = getAvgMarkerDist(markerMapping,f,modelMarkers);
        BOOST_CHECK_LT(avgMarkerDist,100.0f);
    }

    // convert to string
    std::string contentMotion = resultModelMotion2->toXML();
    contentMotion = "<MMM>" + contentMotion;
    contentMotion = contentMotion + "</MMM>";
    MMM::XML::addXMLHeader(contentMotion);

    BOOST_CHECK(!contentMotion.empty());
    MMM::MotionReaderXMLPtr r(new MMM::MotionReaderXML());
    BOOST_CHECK(r);

    // re-create motion
    MMM::MotionPtr m3 = r->createMotionFromString(contentMotion);
    BOOST_CHECK(m3);
    BOOST_CHECK(resultModelMotion2->getNumFrames() == m3->getNumFrames());
    for (unsigned int i=0;i<resultModelMotion2->getNumFrames();i++)
    {
        MMM::MotionFramePtr mf1 = resultModelMotion2->getMotionFrame(i);
        MMM::MotionFramePtr mf2 = m3->getMotionFrame(i);
        BOOST_CHECK(mf1->getRootPose().isApprox(mf2->getRootPose()));
        BOOST_CHECK(mf1->joint.size()==mf2->joint.size());
        BOOST_CHECK(mf1->joint_vel.size()==mf2->joint_vel.size());
        BOOST_CHECK(mf1->joint_acc.size()==mf2->joint_acc.size());
            BOOST_CHECK_CLOSE(mf1->timestep,mf2->timestep,0.01f);
        for (int j=0;j<mf1->joint.size();j++)
        {
            BOOST_CHECK_CLOSE(mf1->joint[j],mf2->joint[j],0.01f);
            BOOST_CHECK_CLOSE(mf1->joint_vel[j],mf2->joint_vel[j],0.01f);
            BOOST_CHECK_CLOSE(mf1->joint_acc[j],mf2->joint_acc[j],0.01f);
        }
    }
}

*/


// BOOST_AUTO_TEST_SUITE_END()
